#define DEBUG
/* 引脚定义 */
#define M0_COUNTER_PIN   2      // 左轮中断计数器引脚
#define M1_COUNTER_PIN   3      // 右轮终端计数器引脚
#define SERVO            4      // 舵机引脚
#define TRIG             5      // 超声波trig引脚
#define ECHO             6      // 超声波echo引脚
#define RECEIVER         7      // RF433 接收器 data 脚
#define M0_0             9      // 左轮 pwm 1
#define M0_1            10      // 左轮 pwm 2
#define M1_0            13      // 右轮 pwm 1
#define M1_1            11      // 右轮 pwm 2

#define LOW_BAT_LED     A0      // 低电压提示灯引脚
#define BATTERY_PIN     A5      // 电量采样引脚

#include "command.h"
#include "auto_run.h"
#include "joystick.h"

float battery;                  // 当前电量值 (10bits ADC)
bool _init = false;

volatile uint16_t 
    left_count   = 0,           // 左轮计数器
    right_count  = 0,           // 右轮计数器
    left_speed   = 0,           // 左邻转速
    right_speed  = 0;           // 右轮转速
    
uint32_t timer   = 0;           // pid 计算周期
uint32_t battery_tick;          // 电池检测时刻
uint32_t m_time1 = 0,           // 左轮计数器采样时间点
         m_time2 = 0;           // 右轮计数器采样时间点

/* 将左轮计数器引脚映射成外部中断号 */
int left_int_pin  = digitalPinToInterrupt (M0_COUNTER_PIN);
/* 将右轮计数器引脚映射成外部中断号 */
int right_int_pin = digitalPinToInterrupt (M1_COUNTER_PIN);

/**
 * 左轮计数器终端子程序
 */
void left_counter () {
    if (millis () - m_time1 > 5) {
        left_count ++;
        m_time1 = millis ();
    }
}

/**
 * 右轮计数器终端子程序
 */
void right_counter () {
    if (millis () - m_time2 > 5) {
        right_count ++;
        m_time2 = millis ();
    }
}

/**
 * 电池电量检测
 */
void check_battery () {
    if (millis () - battery_tick > 1000) {
        battery_tick = millis ();

        float bat = analogRead (BATTERY_PIN);
        if (bat != battery) {
            battery = bat;
            bat /= 102.3;
            Serial.print ("Battery: ");
            Serial.print ("ADC: ");
            Serial.print (battery);
            Serial.print (", converted: ");
            Serial.print (bat);
            Serial.println (" v");
            if (bat < 6.4) {
                conf.state = SC_LOW_BAT;
            }
        }
    }
}

void setup() {
    Serial.begin (9600);
    while (!Serial);
    Serial.println ("start the board...");
    check_battery ();
    
    receiver.enableReceive (digitalPinToInterrupt (RECEIVER));
    servo.attach (SERVO);
    pinMode (TRIG, OUTPUT);
    pinMode (ECHO, INPUT);
    pinMode (LOW_BAT_LED, OUTPUT);

    servo.write (45);
    delay (1000);
    servo.write (135);
    delay (1000);
    servo.write (96);
    delay (1000);

    init_motor (&m0);
    init_motor (&m1);

    read_config ();

    setupPID ();

    Serial.println ("setup done");

    conf.state = -1;
}

/**
 * pid 初始化子程序
 */
void setupPID () {
    pinMode (M0_COUNTER_PIN, INPUT_PULLUP);
    pinMode (M1_COUNTER_PIN, INPUT_PULLUP);

    attachInterrupt (left_int_pin,  left_counter,  FALLING);
    attachInterrupt (right_int_pin, right_counter, FALLING);
}

void loop() {
    if (!_init) {
        setup ();
        _init = true;
    }

    check_battery ();
    if (conf.state == SC_LOW_BAT) {
        pause ();
        conf.state = SC_STOPPED;
//        while (1) {
        for (int i = 0; i < 2; i ++) {
            digitalWrite (LOW_BAT_LED, HIGH);
            delay (300);
            digitalWrite (LOW_BAT_LED, LOW);
            delay (300);
        }
        return;
//        }
    } 

    // enable pid compute
    check_speed ();

    // read command from Serial port
    read_command (&Serial);
    
    switch (conf.mode) {
        case MC_AUTO_RUN:
            auto_run ();
            break;
        case MC_TRACE:
            break;
        case MC_REMOTE_CONTROL:
            break;
        case MC_BLUETOOTH:
            break;
    }
/*
    else {
        action ();
        process_rf433 ();

        while (Serial.available ()) {
            int ch = Serial.read ();
            if (ch != -1) {
                cmd.buff [pos ++] = ch;
    
                if (ch == '\n' && cmd.buff [pos - 2] == '\r') {
                    cmd.length = pos - 1;
                    if (cmd.length > 1) {
                        cmd.buff [cmd.length - 1] = '\0';
                        process_command (&cmd);
                    }
                    memset (cmd.buff, 0, 64);
                    cmd.length = 0;
                    pos = 0;
                }
            }
        }
    }
*/  
}

void check_speed () {
    double temp = millis () - timer;
    if (temp >= 100) {
        detachInterrupt (left_int_pin);
        detachInterrupt (right_int_pin);

        double rpm1 = left_count  * 3000 / temp;
        double rpm2 = right_count * 3000 / temp;
        left_count = 0;
        right_count = 0;
/*
        if (rpm1 > 0) {
            Serial.print ("left speed = ");
            Serial.print (rpm1);
            Serial.println ("rpm");
        }
        if (rpm2 > 0) {
            Serial.print ("right speed = ");
            Serial.print (rpm2);
            Serial.println ("rpm");
        }
*/       

        double delta = (rpm1 - rpm2) * .1;
        double pwm = conf.right_pwm + delta;
        if (pwm < 0) {
            pwm = 0;
        }
        if (pwm > 255) {
            pwm = 255;
        }
        conf.right_pwm = pwm;
/*        
        Serial.print ("delta = ");
        Serial.println (delta);
        show_left_pwm ();
        show_right_pwm ();
*/
        if (conf.mode == MC_AUTO_RUN) {
            move_on ();
        }
   
        timer = millis ();

        attachInterrupt (left_int_pin,  left_counter,  FALLING);
        attachInterrupt (right_int_pin, right_counter, FALLING);
    }
}
